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Abstract 

We consider a system consisting of multiple mobile robots 
in which the user can at any time issue relocation tasks or¬ 
dering one of the robots to move from its current location to 
a given destination location. In this paper, we deal with the 
problem of finding a trajectory for each such relocation task 
that avoids collisions with other robots. The chosen robot 
plans its trajectory so as to avoid collision with other robots 
executing tasks that were issued earlier. We prove that if all 
possible destinations of the relocation tasks satisfy so-called 
valid infrastructure property, then this mechanism is guaran¬ 
teed to always succeed and provide a trajectory for the robot 
that reaches the destination without colliding with any other 
robot. The time-complexity of the approach on a fixed space- 
time discretization is only quadratic in the number of robots. 
We demonstrate the applicability of the presented method on 
several real-world maps and compare its performance against 
a popular reactive approach that attempts to solve the colli¬ 
sions locally. Besides being dead-lock free, the presented ap¬ 
proach generates trajectories that are significantly faster (up 
to 48% improvement) than the trajectories resulting from lo¬ 
cal collision avoidance. 


Introduction 

Consider a future factory where intermediate products are 
moved between workplaces by autonomous robots. The 
worker at a particular workplace calls a robot, puts an ob¬ 
ject to a basket mounted on the robot and orders the robot 
to autonomously deliver the object to another workspace 
where the object will be retrieved by a different worker. 
Clearly, an important requirement on such a system is that 
each robot must be able to avoid collisions with other robots 
autonomously operating in the shared space. The problem 
of avoiding collisions between individual robots can be ap¬ 
proached either from a control engineering perspective by 
employing reactive collision avoidance or from AI perspec¬ 
tive by planning coordinated trajectories for the robots. 

In the reactive approach, the robot follows the short¬ 
est path to its current destination and attempts to resolve 
collision situations as they appear. Each robot periodi¬ 
cally observes positions and velocities of other robots in its 
neighborhood. If there is a potential future collision, the 
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robot attempts to avert the collision by adjusting its im¬ 
mediate heading and velocity. A number of methods have 
been proposed (jVan den Berg, Lin, and Manocha 2008} 
Guy et al. 2009| |Lalish and Morgansen 2012) that prescribe 
how to compute such collision-avoiding velocity in a recip¬ 
rocal multi-robot setting, with the most prominent one be¬ 
ing ORCA (Van Den Berg et al. 2011). These approaches 
are widely used in practice thanks to their computational ef¬ 
ficiency - a collision-avoiding velocity for a robot can be 
computed in a fraction of a millisecond (Van Den Berg et al. 
|201 1[ ). However, these approaches resolve collisions only 
locally and thus they cannot guarantee that the resulting mo¬ 
tion will be deadlock-free and that the robot will always 
reach its destination. 


With a planning approach, the system first searches for 
a set of globally coordinated collision-free trajectories from 
the start position to the destination of each robot. After the 
planning has finished, the robots start following their respec¬ 
tive trajectories. If robots are executing the resulting joint 
plan precisely (or within some predefined tolerance), it is 
guaranteed that the robots will reach their destination while 
avoiding collisions with other robots. It is however known 
that the problem of finding coordinated trajectories for a 
number of mobile objects from the given start configurations 
to the given goal configurations is intractable. More pre¬ 
cisely, the coordination of disks amidst polygonal obstacles 
is NP-hard ( Spirakis and Yap 1984[ ) and the coordination of 
rectangles in a bounded room is PSPACE-hard ( |Hopcroftj 
|Schwartz, and Sharir 1984] ). 

Even though the problem is relatively straightforward to 
formulate as a planning problem in the Cartesian product of 
the state spaces of the individual robots, the solutions can 
be very difficult to find using standard heuristic search tech¬ 
niques as the joint state-space grows exponentially with in¬ 
creasing number of robots. The complexity can be partly 
mitigated using techniques such as ID ( jStandley 2010[ ) or 
M* (Wagner and Choset 2015) that solve independent sub¬ 
conflicts separately, but each such sub-conflict can still be 
prohibitively large to solve because the time complexity of 
the planning is still exponential in the number of robots in¬ 
volved in the sub-conflict. 


Instead, heuristic approaches are often used in practice. 


such as prioritized planning (Erdmann and Lozano-Perez 
|1987|, where the robots are ordered into a sequence and plan 




























one-by-one such that each robot avoids collisions with the 
higher-priority robots. This greedy approach tends to per¬ 
form well in uncluttered environments, but it is in general 
incomplete and often fails in complex environments. 

When the geometric constraints are ignored, com¬ 
plete polynomial algorithms can be designed such as 
Push&Rotate (|de Wilde, ter Mors, and Witteveen 2013) or 
Bibox ( |Surynek 2009 1. These algorithms solve so-called 
“Pebble motion” problem, in which pebbles(robots) move 
on a given graph such that each pebble occupies exactly one 
vertex and no two pebbles can occupy the same vertex or 
travel on the same edge during one timestep. Although this 
model can be useful for coordination of identical robots on 
coarse graphfQ it is not applicable for trajectory coordina¬ 
tion of robots with fine-grained or otherwise rich motion 
models. 

Recall that in our factory scenario, a robot can be assigned 
a task “online” at any time during the operation of the sys¬ 
tem. If one of the classical planners is used, the system 
would have to interrupt all the robots and replan their trajec¬ 
tories each time a new task is assigned, which is clearly un¬ 
desirable. Further, although prioritized planning can be run 
in a decentralized manner (Velagapudi, Sycara, and Scerri 


2010 


Cap et al. 2013), the complete approaches are difficult 


to run without a central solver. 

The main contribution of this paper is COBRA - a novel 
decentralized method for collision avoidance in multi-robot 
systems with online-assigned tasks to individual robots. We 
prove that for robots operating in environments that were 
designed as a valid infrastructures, the method is complete, 
i.e. all tasks are guaranteed to be successfully carried out 
without collision. Furthermore, if a time-extended roadmap 
planner is used for trajectory planning, the algorithm has 
polynomial worst-case time-complexity in the number of 
robots. The applicability of the approach is demonstrated 
using a simulated multi-robot system operating in real-world 
environments. 


Problem Statement 

Consider a 2-d environment (described by a set of obstacle- 
free coordinates W C R 2 ) and a set of points E C W rep¬ 
resenting distinguished locations in the environment called 
endpoints (modeling e.g. workplaces in a factory). The pair 
(W, E) is called and infrastructure. Such an infrastructure is 
populated by n mobile robots with circular bodies indexed 
1 ,... ,n. The radius of the body of robot i is denoted r,, 
the maximum speed robot i can move at is denoted by u™ ax . 
The robots are assumed to be holonomic, i.e. at every time 
step, the robot can select its immediate speed v £ (0, rtf 13 *) 
and heading 6 £ (— 7 r , tt) with the acceleration limits be¬ 
ing neglected. During the operation of the system, robots 
are assigned relocation tasks denoted s —t g requesting the 
chosen robot to move from its current position s £ E to 
the given goal endpoint g £ E. We assume that the robot 

*The graph must be coarse enough so that the bodies of two 
robots “sitting” on two different vertices will never overlap. The 
same has to hold for two robots traveling on different edges of the 
graph. 


cannot be interrupted once it starts executing a particular re¬ 
location task and thus a new relocation task can be assigned 
to a robot only after it has reached the destination of the pre¬ 
viously assigned task. The objective is to find a trajectory 
for each such relocation task such that the robot will reach 
the specified goal without colliding with other robots oper¬ 
ating in the system. Moreover, such trajectories should be 
found in a decentralized fashion without a need for a central 
component coordinating individual robots. 

Notation In the remainder of the paper we will make use 
of the notion of a space-time region: When a spatial object, 
such as the body of a robot, follows a given trajectory, then 
it can be thought of as occupying a certain region in space- 
time T := W x [0,oo). A dynamic obstacle A is then a 
region in such a space-time T. If (x, y, t) £ A, then we 
know that the spatial position (x, y) is occupied by dynamic 
obstacle A at time t. The function 

Ri'i tt) := {(x,y,t) : t £ [0,oo) A (x,y) £ Ri(n(t))} 

maps trajectories of a robot i to regions of space-time that 
the robot i occupies when its center point follows given tra¬ 
jectory tt. As a special case, let i?j^(0) := 0. 

COBRA - General Scheme 

In this section we will introduce Continuous Best-Response 
Approach (COBRA), a decentralized method for trajectory 
coordination in multi-robot systems. The general formu¬ 
lation of the algorithm assumes that each of the robots in 
the system is able to compute an optimal trajectory for itself 
from its current position to a given destination position in the 
presence of moving obstacles without prescribing how such 
a trajectory should be computed. In order to synchronize 
the information flow and ensure that the robots plan their 
trajectory using up-to-date information about the trajecto¬ 
ries of others, robots are using a distributed token-passing 
mechanism (Ghosh 2010[ ) in which the token is used as a 
synchronized shared memory holding current trajectories of 
all robots. We identify a token $ with a set {(oj, 7r*)}, which 
contains at most one tuple for each robot a = 1..., n. Each 
such tuple represents the fact that robot a is moving along 
trajectory n. At any given time the token can be held by only 
one of the robots and only this robot can read and change its 
content. 

A robot newly added to the system tries to obtain the to¬ 
ken and to register itself with a trajectory that stays at its 
initial position forever.After all the robots have been added 
to the system, the user can start with assigning relocation 
tasks to individual robots. 

When a new relocation task is received by robot i, the 
robot requests the token $. When the token is obtained, the 
robot runs a trajectory planner to find a new “best-response” 
trajectory to fulfill the relocation task. The trajectory is re¬ 
quired a) to start at the robot’s current position p at time 
t n ow + ^planning (at the end of the planning window), b) 
to reach the goal position g as soon as possible and remain 
at g and c) to avoid collisions with all other robots follow¬ 
ing trajectories specified in the token. If such a trajectory 
is successfully found, the token is updated with the newly 















Algorithm 1: COBRA - specification for robot i. The 
current time is denoted t now , the maximum time that 
can be spent in trajectory planning is denoted t planning- 

l On registered to the system at position s 

2 

<l> request token ; 

3 

7 r ■£- 7r (i) such that Vf £ [0, oo) : n(f ) = s ; 

4 

$ <— $ U {(*, 7r)} ; 

5 

release token $ ; 

6 On new relocation task s —* g assigned 

7 

<I> request token ; 

8 

assert q is not a destination of another robot; 

9 

<f> «— (<b \ {(i, 7r') : (i, tt') £ $}) ; 

10 

A <- U -Rj(tl); 



11 

^dep ^ ^now H - ^planning > 

12 

7r 4— Best-tra j* (s,td ep ,g, A) ; 

13 

if 7T = 0 then 

14 

report failure 

15 

T* £- U {(*, 7r)} ; 

16 

release token $ ; 

17 

start following n at tdep ; 

18 Function Best-tra j* (s,t s ,g, A) 

19 

return trajectory 7r for robot i that reaches g in 


minimal time such that 

20 

a) 7r(t s ) = s, 

21 

b) 3 t g \/f £ [t g ,oo) : Tr(f') = g. 

22 

c) Ri( 7r) H A = 0 if it exists, 

23 

otherwise return 0; 


generated trajectory and released so that other robots can 
acquire it. Then, the robot starts following the found tra¬ 
jectory. Once the robot successfully reaches the destination, 
it can accept new relocation tasks. The pseudocode of the 
COBRA algorithm is listed in Algorithm[T] 

Theoretical Analysis 

In this section we will derive a sufficient condition under 
which is the presented mechanism complete, i.e. it guaran¬ 
tees that all relocation tasks will be successfully carried out 
without collision. First, observe that in general a robot may 
fail to find a collision-free trajectory to its destination as il¬ 
lustrated in Figure[T] There is, however, a class of infrastruc¬ 
tures, which we call valid infrastructures , where each trajec¬ 
tory planning is guaranteed to succeed and consequently all 
relocation tasks will be carried out without collision. 

Informally, a valid infrastructure has its endpoints dis¬ 
tributed in such a way that any robot standing on an end¬ 
point cannot completely prevent other robots from moving 
between any other two endpoints. In a valid infrastructure, 
a robot is always able to find a collision-free trajectory to 
any other unoccupied endpoint by waiting for other robots 
to reach their destination endpoint, and then by following 
a path around the occupied endpoints, which is in a valid 
infrastructure guaranteed to exist. 

In the following, we will describe the idea more for- 



Figure 1: Scenario in which robot 2 fails to find a trajec¬ 
tory for a relocation task S 2 —> <j-i- First, robot 1 plans 
a trajectory for relocation task si — > g±. It will travel at 
straight line connecting the two points at maximum speed. 
Robot 2 can travel at the same maximum speed as robot 1 
and plans second from S 2 to g%. However, it cannot reach 
the exchange point in time and thus none of the available 
trajectories reaches r /2 without collision with robot 1. The 
trajectory planning by robot 2 fails. 


mally. First, let us introduce the necessary notation. Let 
D(x. r) be a closed disk centered at x with radius r. Then, 
int r A := {x : D( x, r ) C X} is an ?’-interior of a set AT C 

R 2 and ext r A := U D(.r,r) is an r-exterior of a set 

xex 

X £ R 2 . A path is a continuous function p(a) : [0,1] —► R 2 
which represents a curve in R 2 . A trajectory is a function 
7 r(t) : [0, oo) —> R 2 which represents a movement of a 
point in R 2 and time. Given a set X C R 2 , we say that 
a path p is A-avoiding iff Va : p(a) ^ X. Similarly, a 
trajectory 7r is A-avoiding iff Vi : 7r(t) ^ A. The trajec¬ 
tories 7 ti and 7 Tj of robots i and j are said to be collision- 
free iff Vf : |7r,;(f) — 7Tj (f)| > n + Tj. A trajectory 7r is 
g-terminal iff 3t g Vi £ [t g ,oo) : n(t) = g. Token >I> is 
called a) E-terminal iff V(a, it) £ $ : 7r is g-terminal and 
g £ E, and b) collision-free iff V(a,7r), (a', tr') £ *1' : a/ 
a' => 7r and 7r' are collision-free. 

Definition 1. An infrastructure ( W.E ) is called valid 
for circular robots having body radii r-\..... r n if any 
two endpoints a, 6 £ E can be connected by a path 

in workspace intr I W\ U D(e,r) 1, where r = 

V eeE\{«,l) J 

max{ri,... ,r n }. 

In other words, there must exists a path between any two 
endpoints with at least r-clearance with respect to the static 
obstacles and at least 2r-clearance to any other endpoint. 
Figure [2] illustrates the concept of a valid infrastructure. 

The notion of valid infrastructures follows the structure 
typically witnessed in man-made environments that are intu¬ 
itively designed to allow efficient transit of multiple people 
or vehicles. In such environments, the endpoint locations 
where people or vehicles are stopping for longer time are 
separated from the transit area that is reserved for travel be¬ 
tween these locations. 

If we take the road network as an example, the endpoints 
would be the parking places and the system of roads is built 
in such a way that any two parking places are reachable with¬ 
out crossing any other parking place. Similar structure can 
be witnessed in offices and factories. The endpoints would 
be all locations, where people may need to spend longer pe¬ 
riods of time, e.g. surroundings of the work desks or ma- 










(a) Valid infrastructure: 
The workspace W and 
endpoints {ei, e2, e3, 64} 
for robots having radius r 
form a valid infrastructure. 


(b) Non-valid Infrastructure: 
The workspace W and end¬ 
points {ei,e 2 ,ea} do not 
form a valid infrastructure 
because there is no path from 
ei to e 2 with 2r-clearance 
to e 3 for a robot having ra¬ 
dius r. 


Figure 2: Valid and non-valid infrastructure 


chines. As we know from our every day experience, work 
desks and machines are typically given enough free room 
around them so that a person working at a desk or a machine 
does not obstruct people moving between other desks or ma¬ 
chines. We can see that real-world environments are indeed 
often designed as valid infrastructures. 

Completeness in Valid Infrastructures In this section, 
we show that if robots are operating in a valid infrastruc¬ 
ture and use COBRA for trajectory coordination, they will 
successfully carry out all assigned relocation tasks with¬ 
out collisions. Our analysis assumes the following setup of 
the multi-robot system. First, the robots must operate in a 
valid infrastructure (VV. E). When the system is initialized, 
robots are located at distinct initial endpoints of the infras¬ 
tructure. After the initialization is finished, robots start ac¬ 
cepting relocation tasks from some higher-level component, 
which ensures that each destination is an endpoint of the in¬ 
frastructure and two robots will never have simultaneously 
assigned relocation task with the same destination. Each 
robot uses a complete trajectory planner and any trajectory 
generated for a robot, will be precisely followed by robot. 

Proposition 2. If token $ acquired during handling of a new 
task s —i g assigned to robot i (on line^in Algorithm [7]l is 
E-terminal and collision-free, then the subsequent trajectory 
planning succeeds and returns a trajectory that is g-terminal 
and collision-free with respect to other trajectories in <f>. 

Proof Because (W, E) is a valid infrastructure and s , g £ 
E, there exists a ext r ( E \ {s, 5 })-avoiding path p going 
from s to g. All trajectories in $ are E-terminal, which im¬ 
plies that eventually they reach one of the endpoints and stay 
at that endpoint. Consequently, there exists a time point t 
after which all the robots reach their destination endpoints 
and stay there. A g-terminal and collision-free trajectory for 
robot i can be constructed as follows: 


$ during this interval: From the assumption that new re¬ 
location tasks can be assigned only after the previous relo¬ 
cation task has been completed, we have Vi £ [ t now , 00 ) : 
7 r(f) = g'. From the assumption that the start position of 
a new relocation task s must be the same as the goal of the 
robots previous task g', we know Vi > t now : n(t) = s. 
Since <f> is collision-free, all trajectories of robots other 
than j from $ must be avoiding s with r; + Tj clearance, 
where r :j is the radius of the other robot in $. Therefore, 
in time interval [f s , max(f s , i)], robot i will be collision- 
free with respect to other trajectories stored in < 1 ». 

• In time interval i £ [i, 00 ] : tt follows path p until the goal 
position g is reached. The trajectory cannot be in collision 
with any trajectory in $ during this interval: After time t, 
all robots are at their respective goal positions G, which 
satisfy: a) G C E, b) s ^ G, otherwise some of the tra¬ 
jectories would have to be in collision with 7r, which con¬ 
tradicts the finding from the previous point, and c ) g G 

because we assume that two robots cannot have simulta¬ 
neously relocation tasks with the same destination. We 
know that the path p avoids regions ext 2 r (E \ {s,g}) 
and thus the trajectory cannot be in collision with any 
other trajectory in $ during the interval t, 00 ). 

Such a trajectory can always be constructed and thus any 
single-robot complete trajectory planning method must find 
it. □ 

Proposition 3. Every time ( I> is acquired by a robot during 
new relocation task handling (line [7] in Algorithm |7J, <1> is 
E-terminal and collision-free. 

Proof. By induction on <l>. Take an arbitrary sequence of 
individual robots acquiring the token <1» and updating it. Let 
the induction assumption be: Whenever $ is acquired by a 
robot to handle a new relocation task, $ is E-terminal and 
collision-free. 

Base case: When the last robot registers itself, all robots 
have a trajectory in « 1 > that remains at their start positions for¬ 
ever. Since the start position are assumed to be distinct and 
at least 2 r apart, then the <1» is collision-avoiding. Further, <1» 
is trivially E-terminal because after the last robot registers, 
all robots stay at their initial position, which is an endpoint. 

Inductive step: From the inductive assumption, we know 
that $ acquired at line [7] in Algorithm 1 is E-terminal and 
collision-free. Using Proposition^ we see that for any relo¬ 
cation task s —y g, the algorithm will find a trajectory tt that 
is g-terminal and collision-free with respect to other trajec¬ 
tories in <1>. From our assumption, the destination g £ E. By 
adding trajectory tt to T at line[l5]in Algorithm[T| the token 
$ remains E-terminal and collision-free. □ 

Theorem 4. If a team of robots operates in a valid infras¬ 
tructure (W, E) and the COBRA algorithm is used to coor¬ 
dinate relocation tasks between the endpoints of the infras¬ 
tructure, then all relocation tasks will be successfully car¬ 
ried out without collision. 


• In time interval t £ [t s , max(f S) t)] : n(t) = s. The tra¬ 
jectory cannot be in collision with any other trajectory in 


Proof. By contradiction. Assume that a) there can be a re¬ 
location task s —> g assigned to robot i that is not carried 



out successfully or b) two robot collide at some time point 
during the operation of the system. 

Case A: A relocation task s g assigned to a robot i has 
not been successfully completed. We assume that robots are 
able to perfectly follow any given trajectory n. Therefore 
either n is not (^-terminal or robot i collided with another 
robot during the execution of the trajectory. From Propo¬ 
sition [2] and [3] we know that the Best-tra j, routine will 
always return a ^-satisfactory trajectory. The possibility that 
the robot collided while carrying out a relocation task is cov¬ 
ered by Case B and is shown to be impossible. Thus, the 
relocation task s g assigned to robot i will be completed 
successfully. 

Case B: Robots % and j collide. Since we assume perfect 
execution of trajectories, it must be the case that there is 7 r, 
and 7 Tj that are not collision-free. Since the collision relation 
is symmetrical, w.l.o.g., it can be assumed that 7 tj was added 
to <f> later than tt,. This implies that tt, was in the token $ 
when 7 Tj was generated within Best-tra jj routine. How¬ 
ever, this would imply that the trajectory planning returned 
a trajectory that is not collision-free with respect to trajec¬ 
tories in <h, which is a contradiction with constraints used 
during the trajectory planning. Thus, robots i and j do not 
collide. 

We can see that neither case A nor case B can be achieved 
and thus all relocation tasks are carried out without any col¬ 
lision. □ 


COBRA with a Time-extended Roadmap 
Planner 

In the previous section we have presented the general 
scheme of COBRA that assumes that every robot is able 
to find an optimal best-response trajectory for itself using 
an arbitrary complete algorithm. In practice, such a plan¬ 
ning would be often done via graph search in a discretized 
representations of the static workspace. In this section, we 
will analyze the properties of COBRA when all robots use 
a roadmap-based planner to plan their best-response trajec¬ 
tory. 

The function Best-tra jj (s,t s ,g, A) used on line [12] 
in Algorithm [I] returns an optimal trajectory for a particu¬ 
lar robot i from s to g starting at time t s that avoids space- 
time regions A occupied by other robots. We will now as¬ 
sume that the robots use a time-extended roadmap planner 
to compute such a trajectory. The planner takes a graph 
representation of the static workspace and extends it with 
a discretized time dimension. The resulting time-extended 
roadmap is subsequently searched using Dijkstra’s shortest- 
path algorithm (possibly with some admissible heuristic). 
The time-extended roadmap planner can be implemented as 
follows. Let us have a “roadmap” graph G = (V. L) repre¬ 
senting an arbitrary discretization of the static workspace W, 
where V C W represent the discretized positions from VV 
and L C V x V represents possible straight-line transitions 
between the vertices. The time-extended graph G = (V, L) 
for robot i with time step St starting at position s at time t s 
is recursively defined as follows: 


t' 


(s,t s ) i A => (s,t s ) G V 

and 


V(u,f) G V A (ui,u 2 ) G L : 


\V2 - tJjJ 

St ■ v™ ax 


St A line ((iq, f), (u 2 , t')) D A 


(1) 


0 (2) 


(V2,t') G V A ((vi,t), {v 2 ,t')) G L, 

where [■] represents ceiling, |-| is the Euclidean norm, 
and line(x, y) represents the set of points lying on the 
line x —t y. It is important to realize that because we 
force each space-time edge to end a time that is a multi¬ 
ple of <5t, some of the edges may in fact be traveled at a 
slower speed than 77 ™ ax . The best-response trajectory is 
then constructed by finding the shortest path in G start¬ 
ing from the vertex ( s,t s ) to a goal vertex that satisfies 
(g, t g ) A line ((g, t g ), (g, 00 )) n A = 0. 

Valid Infrastructure Roadmap 

The notion of a valid infrastructure can be extended to dis¬ 
cretized representations of the robots’ common workspace 
as follows. 

Definition 5. A graph G = ( ’V., L) is a roadmap for a 
valid infrastructure (W, E) and robots with radii rq.... r n 
if E C V and any two different endpoints a. b G E can be 
connected by a path p = li ,..., 1^ in graph G such that 

V line(Zi) C int P (W \ U D(e,f)] , 

i=l,...,fc \ e£E\{a,b} J 

where r = max{r 1; ..., r„}. 

Analogically to the valid infrastructure, we require that 
the roadmap contains a path that connects any two endpoints 
with at least f clearance to static obstacles and 2r clearance 
to other endpoints. 

Theoretical Analysis 

In this section, we will show that COBRA with a time- 
extended roadmap planner operating on a roadmap for valid 
infrastructure is complete and the trajectory for any reloca¬ 
tion task will be computed in time quadratic to the number 
of robots present in the system. In the following, we will as¬ 
sume that G = (V, L) is a roadmap for a valid infrastructure 

(W,£). 

Theorem 6 . If G = ( V,, L) is a roadmap for a valid infras¬ 
tructure (W, E) and COBRA with a time-extended roadmap 
planner is used to coordinate relocation tasks between the 
endpoints of the infrastructure, then all relocation tasks will 
be successfully carried out without collision. 

Proof. The line of argumentation used to prove the com¬ 
pleteness of COBRA with an arbitrary complete trajectory 
planner (Theorem 4| is also applicable for COBRA with 
time-extended roadmap planner (TERP) - we only need to 
show that Proposition [2] holds when TERP is used to find 






the best-response trajectory. The original argument can be 
adapted as follows: If the robot acquires an E-terminal to¬ 
ken, there is a time-point t, when all robots in $ reach their 
destinations and stay there. A best response trajectory for a 
relocation task s -y g can always be constructed by waiting 

at the robot’s start endpoint until j- t St (the first discrete 

time-point, when all robots from <f> are on an endpoint) and 
then by following the shortest path from s to g on roadmap 
G. ' □ 


Complexity 

In this section we derive the computational complexity of the 
COBRA algorithm when the time-extended roadmap plan¬ 
ner is used for finding the best-response trajectory. We will 
focus on the complexity of trajectory planning for a single 
relocation task and show that if a robot acquires a token with 
trajectories of other robots, all the robots in the token will 
reach their respective destination endpoints in a relative time 
that is bounded by a factor linearly dependent on the num¬ 
ber of robots. Therefore, the state space that needs to be 
searched during the trajectory planning is linear in the num¬ 
ber of robots. 


Assumptions and Notation: Let /( tt) denote the time 
when trajectory tt reaches its destination, i.e. f{tr) := 
min ts.t.t 1 > t : tt (£') = g, g £ E. Further, let A(<b,f) 
denote the set of “active” trajectories from token $ at time 
t defined as A(<I>, t) := {tt : (•, n) £ $ s.t. /( tt) > t}. The 
latest time when all trajectories in token <f> reach their desti¬ 
nation endpoints is given by function F($) := max /( n). 

(•,ir)G$ 

Further, let r denote the duration of the longest possible in¬ 
dividually optimal trajectory between two endpoints by any 
of the robots: 

length of shortest path from ei to e 2 in G 

r = max -. 

ei,e 2 eE min fj ,™ 1 

i=l...n 

Lemma 7. At any time point t we have F(A($>, £))<£ + 
|A($,f)|r. 

Proof Observe that the token only changes during new task 
handling. Let us consider an arbitrary sequence of tasks be¬ 
ing handled by different robots at time-point t\, t 2 .... Ini¬ 
tially, at time t = 0 the token is empty and the inequal¬ 
ity holds trivially: F(0) < 0. We will inductively show 
that the inequality holds after the token update during each 
such task handling, which implies that it also holds for the 
time period until the next update of the token. Now, let us 
take fc th -task handling by robot i with current trajectory tt^, 
that at time r fc obtains token <b fc_1 . By induction hypoth¬ 
esis we have F(A(<b fc_ 1 , r k )) < r k + |A(<b fc_ 1 , r k )\r. 
We know that TTi ^ A{^ k ~ x ,r k ), because the robot can 
only accept new tasks after it has finished the previous 
one and thus f{TTi) < r k . Further, we know that 1) 
Vtt £ <b fe_1 \ A(T> fe - 1 ,r fc ) Vi > r k : tt( f) £ E and 
2 )Vtt £ A(^ k -\T k ) Vi > F(A($ fc - 1 ,r fc )) 7r(f) £ E, 
i.e. “inactive” trajectories are on the endpoints immediately, 
while active trajectories will reach their endpoints and stay 


there after F(A($ fc_1 , r fc ). Then, the robot finds its new 
trajectory n*, which can be in the worst-case constructed 
by waiting at the current endpoint and then following the 
shortest endpoint-avoiding path (which is in infrastructures 
guaranteed to exist and its duration is bounded by r) to 
the destination endpoint. Such a path reaches the destina¬ 
tion in r k < f {tt*) < r k ) + r. Then, the 

robot updates token <b fe «— <b fc_1 \ { m} U {it*}. We know 
that TTi ^ A($ fc_ 1 ,r fc ), but tt* £ A($ fe ,r fe ) and thus 
|A($ fe ,r fe )| = |A(<b fc_ 1 ,r fc )| +1. By rearrangement 

F(A($ fc ” 1 ,r fc )) <r k + |A(<b fc_1 ,r fc )| r 
F(A(<f> fc , r k )) < F(A($ k ~\r k )) + r 
F(A{$ k ,T k )) <r k + |A(<b fc_ 1 ,r fc )| r + r. 
F(A{$ k ,T k )) < r k + \A(<f> k -\T k ) + l\r 
F{A($ k ,T k )) <r k + \A(<f> k ,r k )\r 

□ 


Lemma 8. During each relocation task handling of robot i 
at time t, there is a trajectory tt that reaches the destination 
of the relocation task in time /( tt) <t + nr. 

Proof. It is known that F(A($, t)) < t + |A($, £)| r. Token 
$ is updated in such a way that it contains at most one record 
for each robot. Assume that robot i handles a new relocation 
task. Before planning, robot i removes its trajectory from 
token <1> and thus we have |$| < n— 1. Since |A($, t)\ C <b, 
we have |A(<b,f)| < n — 1 and using Lemma [7] we get 
F(A(<b,£)) < t+(n— l)r, i.e. all other robots will ne at their 
respective destination endpoint at latest time t + (n — 1 )r. 
In the worst case, trajectory tt can be constructed by waiting 
at robot’s i current endpoint until time t + (n — 1 )?’ and then 
following the shortest path to its destination endpoint, which 
can take at most r. Trajectory 7 r thus reaches the destination 
endpoint latest at time t + (n — l)r + r = t + nr. □ 

Theorem. The worst-case asymptotic complexity of a single 
relocation task handling using COBRA with time-extended 
roadmap planning is 0(n 2 v 2 (jj:) 2 r(d + r)), where n is the 
number of robots in the system, v is the number of vertices 
in the roadmap graph, St is the time-discretization step, r is 
the maximum duration of a single relocation when the inter¬ 
actions between robot are ignored, and d is the duration of 
longest space-time edge in time-extended roadmap. 


Proof. Suppose that robot i handles a relocation task s —> g. 
Let v denote the number of vertices of the roadmap graph. 
In the worst case, the time-extended graph can contain all 
vertices from the roadmap G for each time step. The best- 
response trajectory is the shortest path from the initial vertex 
(s,f a ), where t s = t + t pUnning to a goal vertex (g,t 9 ). 
The search algorithm will only need to examine the sub¬ 
graph for the time interval [t s ,t g \, which contains at most 


(tg-ts) 

st 


v vertices. We know t g < t + nr (from Lemma 


81 


and t s > t (because t s = t + ^planning and ^planning > 0 ) 


and thus this subgraph will have at most [ v = ver¬ 
tices for St < nr. This graph first needs to be constructed 










and then searched. Construction: During the construction of 
the space-time subgraph for robot i, each edge e has to be 
checked for collisions with moving obstacles A composed 
of n space-time regions, each representing the disc body of 
another robot j moving along trajectory tt ? (itself composed 
of line segments). Deciding whether e collides with Rj(-Kj) 
can be done in time linear to the number of time steps edge e 
spans, since for each time step r, a sub-segment correspond¬ 
ing to time step r can be extracted both from e and ttj and 
the collision-free property VZ : e(Z) — 7r ;/ (f) < rj + can 
be validated by solving the corresponding quadratic equa¬ 
tion. One edge can be checked in time O(j^n), where d is 
the duration of the edge. There is at most ^ v 2 edges in the 
space-time subgraph and thus it can be constructed in time 
0(n 2 v 2 {ji) 2 dr). Search: The worst-case time-complexity 
of Dijkstra’s shortest path algorithm is 0(N 2 ) , where N is 
the number vertices of the searched graph, which is in our 
case N = nr A . The time-complexity of search is therefore 

O ((^) 2 ) = 0(n 2 v 2 (j^) 2 7' 2 ). By combining construc¬ 
tion and search, we get 0(n 2 v 2 {jj) 2 r(d + r)). □ 

Empirical Analysis 

In this section we compare the performance of COBRA and 
ORCA, a state-of-the art decentralized approach for on-line 
collision avoidance in large multi-robot teams. The two 
algorithms are compared in three real-world environments 
shown in Figure [3] The test environments are valid infras¬ 
tructures. The execution of a multi-robot system is simu¬ 
lated using a multi-robot simulatoi]^] During each simula¬ 
tion, the given number of robots is created and each of them 
is successively assigned 4 relocation tasks to a randomly 
chosen unassigned endpoint. When the simulation is started, 
all the robots are initialized and the first relocation task with 
random destination endpoint is issued. To avoid the initial 
unnatural situation in which all robots would need to plan 
simultaneously, the initial task is issued with a random de¬ 
lay within the interval [0, 30 s], Once the robot reaches the 
destination of its task, a new random destination is generated 
and the process is repeated until the required number of relo¬ 
cation tasks have been generated. For each such simulation, 
we observe whether all robots successfully carried out all as¬ 
signed tasks and the time needed to reach the destination of 
each relocation task. Further, we compute the prolongation 
introduced by collision avoidance as p = t A — Z', where t A 
is a duration of a particular task when an algorithm A is used 
for trajectory coordination, and t! is the time the robot needs 
to reach the destination without collision avoidance simply 
by following the shortest path at the roadmap at maximum 
speed. 

The robots are modeled as circular bodies with radius 
r=50cm that can travel at maximum speed v max =l m/s. The 
relative size of a robot and the environment in which the 
robots operate is depicted in Figure [3] where the red circles 
indicate the size of a robot. The roadmaps are constructed 
as 8 -connected grid with additional vertices and edges added 

2 The simulator and the test instances are available for download 
at http://github.com/mcapino/cobra-icaps2015 



Warehouse 



Office 


Figure 3: Test environments. The infrastructure endpoints 
depicted in red, the roadmap graph shown in gray. 


near the walls to maintain connectivity in narrow passages. 
The non-diagonal edges of the base grid are 130 cm long, 
the diagonal edges are 183 cm long. 

The planning window used by COBRA is /planning = 3 s 
long, yet on average single planning required only around 
0.7 s even on the most challenging instances. The time- 
extended roadmap uses discretization <5Z=650 that conve¬ 
niently splits travel on the non-diagonal edges into 2 time 
steps and diagonal edges into 3 time steps. 


The reactive technique ORCA (Van Den Berg et al. 
[201 If is a control-engineering approach typically used in 
a closed-loop such that at each time instant it selects a 
collision-avoiding velocity vector from the continuous space 
of robot’s velocities that is the closest to the robot’s desired 
velocity. In our implementation, at each time instant the al¬ 
gorithm computes a shortest path from the robots current 
position to its goal on the same roadmap graph as is used for 
trajectory planning by COBRA. The desired velocity vec¬ 
tor then points at this shortest path at the maximum speed. 
When using ORCA, we often witnessed dead-lock situations 
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Figure 4: Results. The bars represent standard deviation. 


during which the robots either moved at extremely slow ve¬ 
locities or even stopped completely. If any of the robots 
did not reach its destination in the runtime limit of 10 mins 
(avg. task duration is less than 33 s), we considered the run 
as failed. 

Results Figure [4] shows the performance of COBRA and 
ORCA in the three test environments. The top row of plots 
shows the success rate of each algorithm on instances with 
increasing number of robots. In accordance with our theo¬ 
retical results, we can see that the COBRA algorithm reli¬ 
ably leads all robots to their assigned destinations without 
collisions. When we tried to realize collision-free operation 
using ORCA, the algorithm led in some cases the robots into 
a dead-lock. The problem was exhibited more often in envi¬ 
ronments with narrow passages as we can see in the success 
rate plot for the Office environment. 

The bottom row of plots shows the average prolongation 
of a relocation task when either COBRA and ORCA is used 
for collision avoidance. The total prolongation introduced 
by COBRA is composed of two components: planning time 
and travel time. When a new task is used, the robot waits 
for fplanning = 3 s in order to plan a collision-free trajec¬ 
tory to the destination of its new task. Only then, the found 
trajectory is traveled by the robot until the desired destina¬ 
tion is reached. The robots controlled by ORCA start mov¬ 
ing immediately because they follow a precomputed policy 
towards the destination of the current task or a collision¬ 
avoiding velocity if a possible collision is detected. Recall 
that ORCA performs optimization in the continuous space 
of robot’s instantaneous velocities, whereas COBRA plans 
a global trajectory on a roadmap graph with a discretized 
time dimension. In the case of simple conflicts, ORCA can 
take advantage of its ability to optimize in the continuous 
space and generates motions where the robots closely pass 


each other, whereas COBRA has to stick to the underlying 
discretization, which does not always allow such close eva¬ 
sions. However, when the conflicts become more intricate, 
the advantages of global planning starts to outweigh the po¬ 
tential loss introduced by space-time discretization. The ex¬ 
act influence of planning in a discretized space-time can be 
best observed by looking at the data point for instances with 
1 robot - these instances do not involve any collision avoid¬ 
ance and thus the prolongation can be attributed purely to the 
discretization of space-time in which the robot plans. Fur¬ 
ther, we can observe that the local collision avoidance is less 
predictable, which is exhibited by the larger standard devia¬ 
tion. 


Conclusion 

We proposed a novel method for on-line multi-robot tra¬ 
jectory planning called Continuous Best-response Approach 
(COBRA) and both formally and experimentally analyzed 
its properties. We have shown that the algorithm has a 
unique set of features - its time complexity is low polyno¬ 
mial (quadratic in the number of robots) and yet it achieves 
completeness in a class of environments called valid infras¬ 
tructures that encompass most human-made environments 
that have been intuitively designed for efficient transport. 
Further, our technique is directly applicable to systems with 
dynamically issued task and can be implemented in a decen¬ 
tralized fashion on heterogeneous robots. We experimen¬ 
tally compared COBRA with a popular reactive technique 
ORCA in three real-world maps using simulation. The re¬ 
sults show that COBRA is more reliable than ORCA and 
in more challenging scenarios, the planning approach gener¬ 
ates trajectories that are up to 48 % faster than ORCA. 

























Acknowledgements Wagner, G., and Choset, H. 2015. Subdimensional ex- 

pansion for multirobot path planning. Artificial Intelligence 
This work was supported by the Grant Agency ot 219' 1 24 

the Czech Technical University in Prague, grant No. 

SGS13/143/OHK3/2T/13 and by the Ministry of Education, 

Youth and Sports of Czech Republic within the grant no. 

LD12044. 


References 

de Wilde, B.; ter Mors, A. W.; and Witteveen, C. 2013. 
Push and rotate: cooperative multi-agent path planning. In 
Proceedings of the 2013 international conference on Au¬ 
tonomous agents and multi-agent systems, 87-94. Interna¬ 
tional Foundation for Autonomous Agents and Multiagent 
Systems. 

Erdmann, M., and Lozano-Perez, T. 1987. On multiple 
moving objects. Algorithmica 2:1419-1424. 

Ghosh, S. 2010. Distributed systems: an algorithmic ap¬ 
proach. CRC press. 

Guy, S. J.; Chhugani, J.; Kim, C.; Satish, N.; Lin, M.; 
Manocha, D.; and Dubey, P. 2009. Clearpath: Highly par¬ 
allel collision avoidance for multi-agent simulation. In Pro¬ 
ceedings of the 2009 ACM SIGGRAPH/Eurographics Sym¬ 
posium on Computer Animation, SCA ’09, 177-187. New 
York, NY, USA: ACM. 

Hopcroft, J.; Schwartz, J.; and Sharir, M. 1984. On the 
complexity of motion planning for multiple independent ob¬ 
jects; pspace- hardness of the "warehouseman’s problem". 
The International Journal of Robotics Research 3(4):76-88. 

Lalish, E., and Morgansen, K. A. 2012. Distributed reactive 
collision avoidance. Autonomous Robots 32(3):207-226. 

Spirakis, P. G., and Yap, C.-K. 1984. Strong np-hardness of 
moving many discs. Inf. Process. Lett. 19( 1):55—59. 

Standley, T. S. 2010. Finding optimal solutions to coopera¬ 
tive pathfinding problems. In Fox, M., and Poole, D., eds., 
AAAI. AAAI Press. 

Surynek, P. 2009. A novel approach to path planning for 
multiple robots in bi-connected graphs. In Proceedings of 
the 2009 IEEE international conference on Robotics and Au¬ 
tomation, ICR A’09, 928-934. Piscataway, NJ, USA: IEEE 
Press. 

Van Den Berg, J.; Guy, S.; Lin, M.; and Manocha, D. 2011. 
Reciprocal n-body collision avoidance. Robotics Research 
3-19. 

Van den Berg, J.; Lin, M.; and Manocha, D. 2008. Recipro¬ 
cal velocity obstacles for real-time multi-agent navigation. 
In Robotics and Automation, 2008. ICRA 2008. IEEE Inter¬ 
national Conference on, 1928-1935. IEEE. 

Cap, M.; Novak, P.; Selecky, M.; Faigl, J.; and Voknnek, 
J. 2013. Asynchronous decentralized prioritized planning 
for coordination in multi-robot system. In Intelligent Robots 
and Systems (IROS), 2013. 

Velagapudi, P.; Sycara, K. P.; and Scerri, P. 2010. Decentral¬ 
ized prioritized planning in large multirobot teams. In IROS, 
4603^1609. IEEE. 



